/******************************************************************************
COPYRIGHT (C) 2013 by IT-Designers GmbH

File:		Line2D.cpp
Created:	25.02.2013
Modified:	04.04.2013
Author:		Max Pejs
******************************************************************************/

#include "Line2D.h"

Line2D::Line2D(Point2d & start, Point2d & end):Start(start), End(end)
{
	this->CalcHesseParams();
}

Line2D::Line2D() : Rho(0.0), Theta(0.0){}

Line2D::Line2D(Vec4i & CVLine)
{
	Start	= Point2d(CVLine[0], CVLine[1]);
	End		= Point2d(CVLine[2], CVLine[3]);
	this->CalcHesseParams();
}

Line2D::Line2D(double rho, double theta, double upperYlim, double lowerYlim):Rho(rho), Theta(theta) // #TOEDIT
{
	double angle_rad = rho * CV_PI / 180.0;
	double tang = tan(angle_rad);

	double y1 = lowerYlim;
	double y2 = upperYlim;

	double x1 = 0.0;
	double x2 = 0.0;

	if(abs(rho - 90.0) == 0.0)
	{
		x1 = theta;
		x2 = theta;
	}

	x1 = -tang * y1 + theta;
	x2 = -tang * y2 + theta; 

	Start = Point2i((int)x1, (int)y1);
	End	  = Point2i((int)x2, (int)y2);
}

void Line2D::PlotParameters(Mat & img, Scalar color /* = CV_RGB(0, 255, 0) */, Point & offset /* = Point(0,0) */)
{
	circle( img, Point((int)(this->Rho), (int)(this->Theta)) + offset, 5, color, 1, 1);
}

void Line2D::CalcHesseParams()
{
	Point2d normVect = End - Start;
	originNormalVect = Point2d(normVect.y, -normVect.x);

	Point2d originPoint (0, 0);
	Point2d intersectPoint;
	this->Intersection(originPoint, originNormalVect, intersectPoint);

	if(intersectPoint.x == 0.0)
	{
		this->Rho = 0.0;
	}
	else if(intersectPoint.y == 0.0)
	{
		this->Rho = 90.0;
	}
	else
	{
		this->Rho = atan2(intersectPoint.y, intersectPoint.x) * 180.0 / PI;
	}
	
	this->Theta = sqrt(intersectPoint.x * intersectPoint.x + intersectPoint.y * intersectPoint.y);
}

Line2D::~Line2D()
{
	int h = 0;
}

bool Line2D::Intersection(Point2d o, Point2d p, Point2d &r)
{
	Point2d x = Start - o;
	Point2d d1 = p - o;
	Point2d d2 = End - Start;

	double cross = d1.cross(d2);

	if (cross == 0.0 )	// there is no intersection
		return false;

	double t2 = x.cross(d1) / cross;
	r = Start + t2 * d2;

	return true;
}

bool Line2D::Intersection(Line2D & line, Point2d &r)
{
	return this->Intersection(line.Start, line.End, r);
}

double Line2D::Distance(Point2d & other)
{
	Point2d end = other + originNormalVect;
	Point2d result;
	Intersection(end, other, result);
	Point2d d = result - other;
	return sqrt(d.x*d.x + d.y*d.y);
}

void Line2D::Transform(Mat & H)
{
	TransformPoint(H, Start);
	TransformPoint(H, End);
	UpdateParams();
}

void Line2D::UpdateParams()
{
	Line2D l(Start, End);
	this->Rho = l.Rho;
	this->Theta = l.Theta;
	this->originNormalVect = l.OriginNormalVect();
}

void Line2D::TransformPoint(Mat & H, Point2d & point2D)
{
	double x = (double)point2D.x;
	double y = (double)point2D.y;
	double z = H.at<double>(2,0) * x + H.at<double>(2,1) * y + 1.0f;

	point2D.x = cvRound( (H.at<double>(0,0) * x + H.at<double>(0,1) * y + H.at<double>(0,2)) / z ); 
	point2D.y = cvRound( (H.at<double>(1,0) * x + H.at<double>(1,1) * y + H.at<double>(1,2)) / z ); 
}

void Line2D::Move(Point2d & offset)
{
	Start += offset;
	End += offset;
	UpdateParams();
}

void Line2D::plotLineToImg(Mat & img, Scalar color)
{
	if(!img.empty())
		line(img,  Start, End, color, 3, CV_AA);
}

double Line2D::Slope() const
{
	Point2d dirVect = End - Start;
	double slope = 0.0;

	if(dirVect.x == 0.0)
	{
		slope = std::numeric_limits<double>::max();
	}
	else if(dirVect.y == 0.0)
	{
		slope = 0.0;
	}
	else
	{
		slope = dirVect.y / dirVect.x;
	}

	return slope;
}

Point2d Line2D::OriginNormalVect(void) const
{
	return originNormalVect;
}

bool Line2D::operator< (const Line2D & r) const
{
	return this->Theta < r.Theta;
}